#!/usr/bin/env python
""" Publishes keys pressed to the teleop_key topic. H is Go Left, K is Go Right, U is Go Faster, and J is Go Slower """

import roslib; roslib.load_manifest('smc_04b')
import rospy
from std_msgs.msg import String
import termios, fcntl, sys, os

def publish_teleop_key_cmd():
	# Initiate 'publish_teleop_key_cmd' node, initiate publisher to 'teleop_key' topic
	teleop_key_pub = rospy.Publisher('teleop_key', String)
	rospy.init_node('publish_teleop_key_cmd')

	# Initiate key entering code w/ termios
	fd = sys.stdin.fileno()

	oldterm = termios.tcgetattr(fd)
	newattr = termios.tcgetattr(fd)
	newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO
	termios.tcsetattr(fd, termios.TCSANOW, newattr)

	oldflags = fcntl.fcntl(fd, fcntl.F_GETFL)
	fcntl.fcntl(fd, fcntl.F_SETFL, oldflags | os.O_NONBLOCK)
	
	# Initiate keyboard input
	keyList = ["U", "u", "J", "j", "H", "h", "K", "k", "S", "s"]		

	print "Navigate with direction keys: "
	print "U - Start/Go Faster "
	print "J - Go Slower/Stop "
	print "H - Turn Right "
	print "K - Turn Left "
	print "S - Emergency Stop"
		

	try:	
		while not rospy.is_shutdown():

			try:
				key = sys.stdin.read(1)
				if (key=="S") or (key=="s"):
					teleop_key_pub.publish(String(key))
					print "Emergency stop"			
				elif (key!="S") and (key!="s"): 
					for option in keyList:			
						if key == option:
							teleop_key_pub.publish(String(key))
							print " published command "
							break
							print "didn't actually break"
					else: # Second clause of the FOR loop
						teleop_key_pub.publish(String("q"))
						rospy.loginfo("Quitting teleoperation")			     
						break
						# Any other key besides H,J,K,U, and S will cause the robot to slow to a complete stop (hopefully gradually). S will cause an 							abrupt stop. This may have negative acceleration effects. 
			except IOError: pass
		rospy.sleep(1.0)
		
	finally:
		termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm)
		fcntl.fcntl(fd, fcntl.F_SETFL, oldflags)
		
if __name__ == '__main__':
	try:
		publish_teleop_key_cmd()
	except rospy.ROSInterruptException: pass
